#include "idt.hpp"
#include "pic.hpp"

namespace system {

    byte pic1_interrupt_offset = 0x08; // \ => default values set by the BIOS
    byte pic2_interrupt_offset = 0x70; // /

    void init_pic(byte pic1_interrupt_offset, byte pic2_interrupt_offset){

        const byte icw1_init = 0x10;
        const byte icw1_icw4 = 0x01;
        const byte icw4_8086 = 0x01;

        outportb(pic1_command, icw1_init|icw1_icw4);
        outportb(pic2_command, icw1_init|icw1_icw4);

        outportb(pic1_data, pic1_interrupt_offset);
        outportb(pic2_data, pic2_interrupt_offset);

        outportb(pic1_data, 4); //IR4 is connected to a slave
        outportb(pic2_data, 2); //slave ID 2

        outportb(pic1_data, icw4_8086);
        outportb(pic2_data, icw4_8086);

        outportb(pic1_data, 0xFF); // \ => Turn all IRQs off
        outportb(pic2_data, 0xFF); // /

        system::pic1_interrupt_offset = pic1_interrupt_offset;
        system::pic2_interrupt_offset = pic2_interrupt_offset;
    }

    void set_irq_handler(byte irq, void (*handler)()){
        if (handler){
            byte interrupt = (irq & 8) ? (irq & 7) + pic2_interrupt_offset : irq + pic1_interrupt_offset;
            set_interrupt_handler(interrupt, handler);
            enable_irq_handler(irq);
        } else {
            disable_irq_handler(irq);
        }
    }

    void unset_irq_handler(byte irq, void (*handler)()){
        byte interrupt = (irq & 8) ? (irq & 7) + pic2_interrupt_offset : irq + pic1_interrupt_offset;
        if (get_interrupt_handler(interrupt) == handler) disable_irq_handler(irq);
    }

    void enable_irq_handler(byte irq){
        byte pic_address = (irq & 8) ? pic2_data : pic1_data;
        outportb(pic_address, inportb(pic_address) &~ (1 << (irq & 7)));
    }

    void disable_irq_handler(byte irq){
        byte pic_address = (irq & 8) ? pic2_data : pic1_data;
        outportb(pic_address, inportb(pic_address) | (1 << (irq & 7)));
    }

}
